#include "PID.h"

//Define parameter
#define epsilon 0.01f
//#define dt 1             //sprawdzanie wartosci co 1s
//#define Kp  4.2f
//#define Ki  0.1f

float PIDcal(float setpoint, float actual_position)
{
	static float integral;
	float error;
	float out;
	//uint16_t output;

	error = setpoint - actual_position;

	if(abs(error) > epsilon)
	{
		integral = integral + error*(12500000/(1+LPC_RIT->RICOMPVAL)); //error * dt (okres przerwania RIT)
	}
	out = ((PVal.kp)*error) + ((PVal.ki)*integral);

	//if(out>128){		//ograniczenie
	//	out = 128;
	//}
//	else{
//		out = (uint16_t) out;
//	}

 return out;
}

float abs(float value){
	return (value>0) ? value : (-value);
}

